import matplotlib.pyplot as plt
import numpy as np
import math

delta = 0.1  #采样间隔
T = 10 #总时间
t_0 =np.arange(0, T, 0.01) #采样前近似连续时间，用于描述真实信号
XK = [0] * len(t_0)
for i in range(0,len(t_0)):
    XK[i] = t_0[i] + 3
K = list(range(1, int(T / delta) + 1))#采样次数
wk = [0] * len(K)  #下面都是设置对应的列表
t = [0] * len(K)
x_measure = [0] * len(K)
x_hat= [0] * len(K)
e1 = [0] * len(K)
e2 = [0] * len(K)

for i in range(0, len(K)):
    t[i] = (K[i] - 1) * delta
    wk[i] = np.random.normal(loc=0.0, scale=5.0, size=None)  #测量噪声
    x_measure[i] = t[i] + 3 + wk[i]  #测量值等于真实值加上测量噪声

#二阶最小二乘滤波器,估计值的计算
m1 = np.array([[4,sum(t),sum(list(map(lambda x:x**2,t)))]
                ,[sum(t),sum(list(map(lambda x:x**2,t))),sum(list(map(lambda x:x**3,t)))]
                ,[sum(list(map(lambda x:x**2,t))),sum(list(map(lambda x:x**3,t))),sum(list(map(lambda x:x**4,t)))]
              ])
                                     #最后一个求和符号里面是求数列个元素平方
w1 = [t*x_measure for t,x_measure in zip(t,x_measure)]
                                     #t和x逐项相乘
t_2 = list(map(lambda x:x**2,t))
w2 = [t_2*x_measure for t_2,x_measure in zip(t_2,x_measure)]
m2 = np.array([[sum(x_measure)],[sum(w1)],[sum(w2)]])

a = (np.linalg.inv(m1)).dot(m2)      #m1求逆再乘以m2
a0 = float(a[0])
a1 = float(a[1])
a2 = float(a[2])
for i in range(0,len(K)):
    x_hat[i] = a2 + a1 * t[i] + a2 * (t[i] ** 2)
    e1[i] = x_hat[i] - x_measure[i]  #估计值与测量值的误差
    e2[i] = x_hat[i] - (t[i] + 3)  #估计值与真实值的误差


plt.xlabel("Time(Sec)")
plt.ylabel("xhat")
plt.plot(t,x_measure,'-o',c = 'r',label = 'measures')
plt.plot(t,x_hat, c="k",label = 'estimates')
plt.legend()
plt.show()

plt.xlabel("Time(Sec)")
plt.plot(t,e1,'-o',label = 'error between estimates and measures')
plt.plot(t,e2,label = 'error between estimates and true')
plt.legend()
plt.show()

plt.xlabel("Time(Sec)")
plt.plot(t_0, XK, c="r",label = 'true',ls="--", lw=2)
plt.plot(t,x_hat,label = 'estimates')
plt.xlim(0,T)
plt.legend()
plt.show()

